% .. _kalman-filter:
%
% Kalman Filter
% ********************************************************************************
%
% Consider a nonlinear system given by the stochastic differential equation
%
% .. math:: \dot{{\bf x}} = {\bf f} ( {\bf x}(t) , t ) + {\bf G} (t) {\bf w} (t)
%
% where
%
% * :math:`{\bf w}` is a white gaussian process. :math:`{\bf w}(t)` ~ :math:`N ( {\bf 0} , {\bf Q}(t) )`
% * :math:`{\bf f} ( {\bf x}(t) , t )` is a function vector of dimension :math:`n`
% * :math:`{\bf x}(t)` is the state vector :math:`(n \times 1)`
% * :math:`{\bf G}` is a known matrix :math:`(n \times s)`
%
% In this example, the plant is represented by the nonlinear model described in :ref:`vehicle-simple-3dof` with a nonlinear tire model described in :ref:`tire-pacejka`.
%
% The mathematical model used in the Kalman Filter algorithm is based on the plant model. Hypothetically, we assume that the designer of the Kalman Filter do not possess full knowledge of the characteristic curve of the tire. Thus, the vehicle model of the algorithm is the same used to represent the real vehicle. However, the tire model used is linear (:ref:`tire-linear`). This way, the system model used in the Kalman Filter algorithm is a nonlinear vehicle model with a linear tire model.
%
% Defining the tire and nonlinear vehicle models with the default parameters. This model will represent the real vehicle system in the simulation environment.
%
% .. code-block:: matlab

TirePlant = TirePacejka;
VehiclePlant = VehicleSimpleNonlinear;
VehiclePlant.tire = TirePlant;

% Lets simulate the vehicle maneuver to be estimated by the Kalman Filter.
%
% The chosen simulation parameters are
%
% .. code-block:: matlab

T = 6;                              % Total simulation time [s]
resol = 50;                         % Resolution
TSPAN = 0:T/resol:T;                % Time span [s]

% Initializing the simulator and simulating.
%
% .. code-block:: matlab

simulatorPlant = Simulator(VehiclePlant, TSPAN);
simulatorPlant.dPSI0 = 0.35;
simulatorPlant.Simulate

% Retrieving state responses
%
% .. code-block:: matlab

XTPlant = simulatorPlant.XT;
YTPlant = simulatorPlant.YT;
PSIPlant = simulatorPlant.PSI;
vTPlant = simulatorPlant.VEL;
ALPHATPlant = simulatorPlant.ALPHAT;
dPSIPlant = simulatorPlant.dPSI;

XOUTPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];

% Generating the graphics of the maneuver.
%
% .. code-block:: matlab

gPlant = Graphics(simulatorPlant);
gPlant.TractorColor = 'r';
gPlant.Frame();

% .. figure:: ../illustrations/frame/KalmanFilterFrame1.svg
%     :align:   center
%     :width: 70%
%
%     Leegend
%
% Now, the model used in thhe Kalman Filter algorithm is presented.
%
% Initializing the tire and vehicle models
%
% .. code-block:: matlab

TireModel = TireLinear;
VehicleModel = VehicleSimpleNonlinear;
VehicleModel.tire = TireModel;

% Note that the model used in the KF algorithm is the same as the one used to represent the plant except by the tire model. The plant has a nonlinear tire model while the KF model has a linear model.
%
% Simulating the KF model with the same time span.
%
% .. code-block:: matlab

simulatorModel = Simulator(VehicleModel, TSPAN);
simulatorModel.dPSI0 = 0.35;
simulatorModel.Simulate;

% Retrieving states
%
% .. code-block:: matlab

XTModel = simulatorModel.XT;
YTModel = simulatorModel.YT;
PSIModel = simulatorModel.PSI;
vTModel = simulatorModel.VEL;
ALPHATModel = simulatorModel.ALPHAT;
dPSIModel = simulatorModel.dPSI;

% The maneuver generated by the simplified model (with the same initial conditions) is illustrated below
%
% .. code-block:: matlab

gModel = Graphics(simulatorModel);
gModel.TractorColor = 'g';
gModel.Frame();

% .. figure:: ../illustrations/frame/KalmanFilterFrame2.svg
%     :align:   center
%     :width: 70%
%
%     Leegend
%
% Comparing state response of both models. The following lines plot the time response of each state of the model.
%
% .. code-block:: matlab

f1 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XTPlant,'r')
plot(TSPAN,XTModel,'r--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('Distance X [m]')

f2 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,YTPlant,'g')
plot(TSPAN,YTModel,'g--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('Distance Y [m]')

f3 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,PSIPlant,'b')
plot(TSPAN,PSIModel,'b--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('PSI [rad]')

f4 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,vTPlant,'c')
plot(TSPAN,vTModel,'c--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('vT [m/s]')

f5 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ALPHATPlant,'m'),
plot(TSPAN,ALPHATModel,'m--'),
legend('Plant','Model')
xlabel('Time [s]')
ylabel('ALPHAT [rad/s]')

f6 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,dPSIPlant,'k')
plot(TSPAN,dPSIModel,'k--')
legend('Plant','Model')
xlabel('Time [s]')
ylabel('dPSI [rad/s]')

% The generated figures can be seen below.
%
% .. figure:: ../illustrations/plot/KalmanFilterFig1.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig2.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig3.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig4.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig5.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig6.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% Comparing longitudinal and lateral acceleration. The acceleration of both models are calculated as
%
% .. code-block:: matlab

saidasPlant = [XTPlant YTPlant PSIPlant vTPlant ALPHATPlant dPSIPlant];
matDerivEstadosPlant = zeros(size(saidasPlant));
for i = 1:size(saidasPlant,1)
    auxil = simulatorPlant.Vehicle.Model(1,saidasPlant(i,:),TSPAN);
    matDerivEstadosPlant(i,:) = auxil';
end

dXTPlant = matDerivEstadosPlant(:,1);
dYTPlant = matDerivEstadosPlant(:,2);
dPSIPlant = matDerivEstadosPlant(:,3);
dvTPlant = matDerivEstadosPlant(:,4);
dALPHATPlant = matDerivEstadosPlant(:,5);
ddPSIPlant = matDerivEstadosPlant(:,6);

ddXPlant = dvTPlant.*cos(PSIPlant + ALPHATPlant) - vTPlant.*(dPSIPlant + dALPHATPlant).*sin(PSIPlant + ALPHATPlant);
ddYPlant = dvTPlant.*sin(PSIPlant + ALPHATPlant) + vTPlant.*(dPSIPlant + dALPHATPlant).*cos(PSIPlant + ALPHATPlant);

ACELNumPlant = [(ddXPlant.*cos(PSIPlant) - ddYPlant.*sin(PSIPlant))  (-ddXPlant.*sin(PSIPlant) + ddYPlant.*cos(PSIPlant))];

saidasModel = [XTModel YTModel PSIModel vTModel ALPHATModel dPSIModel];
matDerivEstadosModel = zeros(size(saidasModel));
for i = 1:size(saidasModel,1)
    auxil = simulatorModel.Vehicle.Model(1,saidasModel(i,:),TSPAN);
    matDerivEstadosModel(i,:) = auxil';
end

dXTModel = matDerivEstadosModel(:,1);
dYTModel = matDerivEstadosModel(:,2);
dPSIModel = matDerivEstadosModel(:,3);
dvTModel = matDerivEstadosModel(:,4);
dALPHATModel = matDerivEstadosModel(:,5);
ddPSIModel = matDerivEstadosModel(:,6);

ddXModel = dvTModel.*cos(PSIModel + ALPHATModel) - vTModel.*(dPSIModel + dALPHATModel).*sin(PSIModel + ALPHATModel);
ddYModel = dvTModel.*sin(PSIModel + ALPHATModel) + vTModel.*(dPSIModel + dALPHATModel).*cos(PSIModel + ALPHATModel);

ACELNumModel = [(ddXModel.*cos(PSIModel) - ddYModel.*sin(PSIModel))  (-ddXModel.*sin(PSIModel) + ddYModel.*cos(PSIModel))];

% The longitudinal and lateral acceleration are plotted with
%
% .. code-block:: matlab

f7 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,ACELNumPlant(:,1),'r')
plot(TSPAN,ACELNumPlant(:,2),'g')
plot(TSPAN,ACELNumModel(:,1),'r--')
plot(TSPAN,ACELNumModel(:,2),'g--')
xlabel('time [s]')
ylabel('acc. [m/s]')
l = legend('AX Plant','AY Plant','AX Model','AY Model');
set(l,'Location','NorthEast')

% The resulting comparison is illustrated in the figure below
%
% .. figure:: ../illustrations/plot/KalmanFilterFig7.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% Comparing the maneuvers of the plant and the model.
%
% .. code-block:: matlab

gPlant.Frame();
gModel.Frame('nextplot','add');

% .. figure:: ../illustrations/frame/KalmanFilterFrame3.svg
%     :align:   center
%     :width: 70%
%
%     Leegend
%
% The general expression of the linearized model is optained using the symbolic processor of Octave/Matlab.
%
% Defining symbols
%
% .. code-block:: matlab

syms XT YT PSI vT ALPHAT dPSI mT IT a b K

% The tire slip angles are
%
% .. code-block:: matlab

ALPHAF = atan((vT * sin(ALPHAT) + a * dPSI)/(vT * cos(ALPHAT))); % Front
ALPHAR = atan((vT * sin(ALPHAT) - b * dPSI)/(vT * cos(ALPHAT))); % Rear

% The Lateral forces
%
% .. code-block:: matlab

FyF = -K*ALPHAF;                    % Front
FyR = -K*ALPHAR;                    % Rear

% State equations
%
% .. code-block:: matlab

f1e = vT * cos(ALPHAT + PSI);
f2e = vT * sin(ALPHAT + PSI);
f3e = dPSI;
f4e = (FyF * sin(ALPHAT) + FyR * sin(ALPHAT))/(mT);
f5e = (FyF * cos(ALPHAT) + FyR * cos(ALPHAT) - mT * vT * dPSI) / (mT * vT);
f6e = (FyF * a - FyR * b) / IT;

f = [f1e ; f2e ; f3e ; f4e ; f5e ; f6e];

% State vector
%
% .. code-block:: matlab

States = [XT ; YT ; PSI ; vT ; ALPHAT ; dPSI];

% The linearized system is written as
%
% .. math:: \dot{\bf x} = {\bf F} {\bf x}
%
% where :math:`{\bf F}` is the dynamic matrix of the linear model obtained after the truncation of the expansion of the Taylor series. Thus, the :math:`{\bf F}` matrix is
%
% .. math:: {\bf F} = \left[ \frac{\partial f_i}{\partial x_j} \right]_{n \times n}
%
% where :math:`i` e :math:`j` indicate the equations and state variables used in the calculations of at the position :math:`(i,j)` of the jacobian matrix.
%
% Calculating the jacobian matrix.
%
% .. code-block:: matlab

F = jacobian(f,States);
F = simplify(F);

% Defining a matlab function to retrieve the numerical value of F. This way is faster than using "subs".
%
% .. code-block:: matlab

Ffunc = matlabFunction(F,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});

% Hypothetically, the measured quantities are:
%
% * Position X
% * Position Y
% * Yaw rate
% * Longitudinal acceleration
% * Lateral acceleration
%
% For this, we use:
%
% .. math:: \dot{x} = v_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right)
%
% .. math:: \dot{y} = v_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right)
%
% Longitudinal and lateral acceleration are
%
% .. math:: \ddot{x} = \dot{v}_{\rm T} \cos \left( \psi + \alpha_{\rm T} \right) - v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \sin \left( \psi + \alpha_{\rm T} \right)
%
% .. math:: \ddot{y} = \dot{v}_{\rm T} \sin \left( \psi + \alpha_{\rm T} \right) + v_{\rm T} \left( \dot{\psi} + \dot{\alpha}_{\rm T} \right) \cos \left( \psi + \alpha_{\rm T} \right)
%
% Thus
%
% .. code-block:: matlab

ddX = f4*cos(PSI + ALPHAT) - vT*(dPSI + f5)*sin(PSI + ALPHAT);
ddY = f4*sin(PSI + ALPHAT) + vT*(dPSI + f5)*cos(PSI + ALPHAT);

% The equations above describe the dynamics from :math:`\{ O {\bf i} {\bf j} {\bf k} \}`. In the moving reference frame :math:`\{ O {\bf t}_x {\bf t}_y {\bf t}_z \}` the equations are
%
% .. math:: {\bf a} = \left( \ddot{x} \cos \psi - \ddot{y} \sin \psi \right) {\bf t}_x + \left( - \ddot{x} \sin \psi + \ddot{y} \sin \psi \right) {\bf t}_y
%
% Thus,
%
% .. code-block:: matlab

ACEL = [ddX*cos(PSI) - ddY*sin(PSI) ; -ddX*sin(PSI) + ddY*cos(PSI)];
ACEL = simplify(ACEL);

% The nonlinear observation equation is given by
%
% .. math:: {\bf z}_k = {\bf h} ({\bf x}_k) + {\bf v}_k
%
% with :math:`{\bf v}_k $ ~ $ N ( {\bf 0} , {\bf R}_k )`.
%
% The linear version is given by
%
% .. math:: {\bf z}_k = {\bf H} {\bf x}_k + {\bf v}_k
%
% where
%
% .. math:: {\bf H} = \left[ \frac{\partial h_i}{\partial x_j} \right]_{m \times n}
%
% that is, the output matrix :math:`{\bf H}` is the jacobian matrix of the ACEL equation in relation to the system states.
%
% .. code-block:: matlab

medNonlinear = [XT ; YT ; f6 ; ACEL];

H = jacobian(medNonlinear,States);
H = simplify(H);

% Again, defining matlab functions to retrieve the numerical value of medNonlinearfunc and H.
%
% .. code-block:: matlab

medNonlinearfunc = matlabFunction(medNonlinear,'Vars',{XT YT PSI vT ALPHAT dPSI mT IT a b K});

Hfunc = matlabFunction(H,'Vars',{PSI vT ALPHAT dPSI mT IT a b K});

% Implementing the algorithm
%
% Noise matrix
%
% .. code-block:: matlab


G = eye(6);                         % Identity matrix (6 x 6)

% Covariance matrices
%
% .. code-block:: matlab

Q = eye(6);
R = eye(5);

% Matrix
%
% .. code-block:: matlab

P0 = eye(6);

% Retrieving the initial conditions defined above.
%
% .. code-block:: matlab

X0Num = simulatorModel.X0;
Y0Num = simulatorModel.Y0;
PSI0Num = simulatorModel.PSI0;
VEL0Num = simulatorModel.V0;
ALPHAT0Num = simulatorModel.ALPHAT0;
dPSI0Num = simulatorModel.dPSI0;

x0 = [ X0Num ; Y0Num ; PSI0Num ; VEL0Num ; ALPHAT0Num ; dPSI0Num ];
x0 = zeros(6,1);
x0(4)=20;

% Retrieving the vehicle parameters.
%
% .. code-block:: matlab

mTNum = VehicleModel.mT;
ITNum = VehicleModel.IT;
aNum = VehicleModel.a;
bNum = VehicleModel.b;
KNum = TireModel.k;

parameters = [mTNum ITNum aNum bNum KNum];

% Sample time
%
% .. code-block:: matlab

sampleTime = 0.1;

t = 0:sampleTime:T;                 % Array of observation instants

% Preallocation
%
% .. code-block:: matlab

XOUTopt = zeros(length(t) + 1,length(States));  % Estimate of the states after update
Popt = zeros(length(t) + 1,1);                  % Covariance matrix after update
Pantes = zeros(length(t) + 1,1);                % Covariance matrix before update
KKalmanopt = ones(length(t) + 1,1);             % Kalman gain

% Using the first values
%
% .. code-block:: matlab

XOUTopt(1,:) = x0';
XOUTantes(1,:) = x0';
Pantes(1,1) = norm(P0);
Popt(1,1) = norm(P0);

% Error distribution
%
% .. code-block:: matlab

pesos = [5; 5; 0.1; 0.5; 0.5];

% Iterations
%
% .. code-block:: matlab

for j = 1:length(t)
    % Index varying through all instants of observation

    % Time span for the propagation phase
    tspan = t(j):sampleTime/100:t(j)+sampleTime;

    % Measures of the interation
    z = [interp1(TSPAN,XTPlant(:,1),t(j)) ;
         interp1(TSPAN,YTPlant(:,1),t(j)) ;
         interp1(TSPAN,ddPSIPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,1),t(j)) ;
         interp1(TSPAN,ACELNumPlant(:,2),t(j))];

    z = z + pesos.*(rand(5,1)-0.5);

    Fnum = Ffunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);
    Hnum = Hfunc(PSI0Num,VEL0Num,ALPHAT0Num,dPSI0Num,mTNum,ITNum,aNum,bNum,KNum);

    % Propagation cycle
    % Transforming matrix PMat0 (6 x 6) in P0 (1 x 36)
    P0 = reshape(P0',[1 36]);

    [TOUT,Pout] = ode45(@(t,P) IntCov(t,P,Fnum,G,Q),tspan,P0);

    Pmatrix = reshape(Pout(end,:),[6 6])';

    simulatorKalman = Simulator(VehicleModel, tspan);

    % Initial conditions
    simulatorKalman.X0 = x0(1);
    simulatorKalman.Y0 = x0(2);
    simulatorKalman.PSI0 = x0(3);
    simulatorKalman.V0 = x0(4);
    simulatorKalman.ALPHAT0 = x0(5);
    simulatorKalman.dPSI0 = x0(6);

    % Simulation
    simulatorKalman.Simulate()

    XTKalman = simulatorKalman.XT;
    YTKalman = simulatorKalman.YT;
    PSIKalman = simulatorKalman.PSI;
    vTKalman = simulatorKalman.VEL;
    ALPHATKalman = simulatorKalman.ALPHAT;
    dPSIKalman = simulatorKalman.dPSI;

    XOUTKalman = [XTKalman YTKalman PSIKalman vTKalman ALPHATKalman dPSIKalman];

    % Update cycle

    ACELKalman = medNonlinearfunc(XTKalman(end),
                                  YTKalman(end),
                                  PSIKalman(end),
                                  vTKalman(end),
                                  ALPHATKalman(end),
                                  dPSIKalman(end),
                                  mTNum,
                                  ITNum,
                                  aNum,
                                  bNum,
                                  KNum);

    KKalman = Pmatrix*Hnum' / (Hnum*Pmatrix*Hnum' + R);

    XKalman = XOUTKalman(end,:)' + KKalman*(z - ACELKalman);
    PKalman = Pmatrix - KKalman*Hnum*Pmatrix;

    x0 = XKalman;
    P0 = PKalman;

    XOUTopt(j+1,:) = XKalman';
    XOUTantes(j+1,:) = XOUTKalman(end,:);
    Popt(j+1) = norm(PKalman);
    Pantes(j+1) = norm(Pmatrix);
    KKalmanopt(j+1) = norm(KKalman);

end

% Comparison
%
% .. code-block:: matlab

f8 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,1),'r')
p = plot(t(2:end),XOUTopt(2:end-1,1),'r--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('x [m]')

f9 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,2),'g')
p = plot(t(2:end),XOUTopt(2:end-1,2),'g--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('y [m]')

f10 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,3),'b')
p = plot(t(2:end),XOUTopt(2:end-1,3),'b--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('PSI [rad]')

f11 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,4),'c')
p = plot(t(2:end),XOUTopt(2:end-1,4),'c--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('vT [m/s]')

f12 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,5),'m')
p = plot(t(2:end),XOUTopt(2:end-1,5),'m--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('ALPHAT [rad/s]')

f13 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
plot(TSPAN,XOUTPlant(:,6),'k')
p = plot(t(2:end),XOUTopt(2:end-1,6),'k--');
set(p,'Marker','*','MarkerSize',3)
l = legend('Plant','Estim');
set(l,'Location','SouthEast')
xlabel('tempo [s]')
ylabel('dPSI [rad/s]')

% Cov. do erro
%
% .. code-block:: matlab

f14 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
p = plot(t(2:end),KKalmanopt(2:end-1),'r');
set(p,'Marker','*','MarkerSize',3)
ylabel('ganho de kalman')
xlabel('tempo [s]')

f15 = figure;
ax = gca;
set(ax,'NextPlot','add','Box','on','XGrid','on','YGrid','on')
p = plot(t(2:end),Popt(2:end-1),'r');
set(p,'Marker','*','MarkerSize',3)
p = plot(t(2:end),Pantes(2:end-1),'g');
set(p,'Marker','*','MarkerSize',3)
l = legend('+','-');
set(l,'Location','NorthEast')
ylabel('cov. erro')
xlabel('tempo [s]')

% The resulting plots can be seen below
%
% .. figure:: ../illustrations/plot/KalmanFilterFig8.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig9.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig10.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig11.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig12.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig13.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig14.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% .. figure:: ../illustrations/plot/KalmanFilterFig15.svg
%     :align:   center
%     :width: 40%
%
%     Leegend
%
% Estimated trajectory
%
% .. code-block:: matlab

gKalman = Graphics(simulatorKalman);
gKalman.Simulator.TSpan = t;
gKalman.Simulator.XT = XOUTopt(1:end-1,1);
gKalman.Simulator.YT = XOUTopt(1:end-1,2);
gKalman.Simulator.PSI = XOUTopt(1:end-1,3);
gKalman.Simulator.VEL = XOUTopt(1:end-1,4);
gKalman.Simulator.ALPHAT = XOUTopt(1:end-1,5);
gKalman.Simulator.dPSI = XOUTopt(1:end-1,6);
gKalman.TractorColor = 'b';

gPlant.Frame();
gKalman.Frame('nextplot','add');

% .. figure:: ../illustrations/frame/KalmanFilterFrame4.svg
%     :align:   center
%     :width: 70%
%
%     Leegend
